{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from math import sin, cos, tan, pi, sqrt\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "l = 0.1  # m\n",
    "delta_t = 1.  # sec"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def sample_normal_distribution(b):\n",
    "    \"\"\"\n",
    "    sample normal distribution with 0 mean, variance b\n",
    "    \"\"\"\n",
    "    rand = np.random.uniform(low=-1.0, high=1.0, size=12)\n",
    "    return b * np.sum(rand) / 6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def sample_bicycle_motion_model(x_tm1, u):\n",
    "    \"\"\"\n",
    "    Sample bicycle's velocity motion model\n",
    "    Input:\n",
    "        x_tm1 (np.ndarray) - shape 3,: previous pose\n",
    "        u (np.ndarray) - shape 2,: command (v, alpha)\n",
    "    \"\"\"\n",
    "    v, alpha = u\n",
    "    x, y, theta = x_tm1\n",
    "    \n",
    "    # perturbe command signal\n",
    "    sigma_v = 0.5 * v\n",
    "    sigma_alpha = 25. * pi / 180.\n",
    "    v_hat = v + sample_normal_distribution(sigma_v**2)\n",
    "    alpha_hat = alpha + sample_normal_distribution(sigma_alpha**2)\n",
    "    \n",
    "    # generate sample\n",
    "    x_p = x + v_hat * delta_t * (cos(theta) - tan(alpha_hat) * sin(theta))\n",
    "    y_p = y + v_hat * delta_t * (sin(theta) + tan(alpha_hat) * cos(theta))\n",
    "    theta_p = theta + v_hat * delta_t * tan(alpha_hat) / l\n",
    "    \n",
    "    return np.array([x_p, y_p, theta_p])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# initial pose\n",
    "x_tm1 = np.array([0., 0., pi/4])\n",
    "u_t = np.array([sqrt(2.), 0.])\n",
    "sample = [sample_bicycle_motion_model(x_tm1, u_t) for i in range(500)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_sample(x_t, length=0.05):\n",
    "    \"\"\"\n",
    "    Plot a pose\n",
    "    \"\"\"\n",
    "    x, y, theta = x_t\n",
    "    x_tip = x + length * cos(theta)\n",
    "    y_tip = y + length * sin(theta)\n",
    "    plt.plot([x, x_tip], [y, y_tip], 'k-')\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.figure()\n",
    "plt.xlim(0., 2.)\n",
    "plt.ylim(0., 2.)\n",
    "plt.gca().set_aspect('equal', adjustable='box')\n",
    "plt.xlabel(\"x (m)\")\n",
    "plt.ylabel(\"y (m)\")\n",
    "for x_t in sample:\n",
    "    plot_sample(x_t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
